// --------------------------------------------------------------------------------------------------------------------
// <copyright file="BoxRegion.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Numerics
{
    using System.Runtime.Serialization;

    /// <summary>
    /// A basic region represented by a box
    /// </summary>
    [DataContract]
    public class BoxRegion
    {
        /// <summary>
        /// the length (x)
        /// </summary>
        [DataMember]
        private readonly double length;

        /// <summary>
        /// the width (y)
        /// </summary>
        [DataMember]
        private readonly double width;

        /// <summary>
        /// the height (z)
        /// </summary>
        [DataMember]
        private readonly double height;

        /// <summary>
        /// the pose of the box, reference frame of the box is
        /// located at the center of the box
        /// </summary>
        [DataMember]
        private readonly Pose pose;

        /// <summary>
        /// Initializes a new instance of the <see cref="BoxRegion" /> class.
        /// </summary>
        /// <param name="length">the length (x-dimension) of the box</param>
        /// <param name="width">the width (y-dimension) of the box</param>
        /// <param name="height">the height (z-dimension) of the box</param>
        /// <param name="pose">the pose of the box, coordinate system located at the geometric center of the box</param>
        public BoxRegion(double length, double width, double height, Pose pose)
        {
            if (length <= 0 || width <= 0 || height <= 0)
            {
                string msg = string.Format("Expect positive values for dimensions while the given dimensions are length={0}, width={1}, height={2}", length, width, height);
                throw new System.ArgumentException(msg);
            }

            this.length = length;
            this.width = width;
            this.height = height;
            this.pose = pose;
        }

        /// <summary>
        /// given a point in space, is it contained within this box region?
        /// </summary>
        /// <param name="pointInWorld">the point to check</param>
        /// <returns>whether the point is contained in this box region</returns>
        public bool Contains(Vector3 pointInWorld)
        {
            // transform the point to check to the local coordinate system of the box
            // for the ease of checking
            Matrix4 boxInWorld = HomogenousTransform.CreateFromPose(this.pose);
            Matrix4 worldInBox = HomogenousTransform.Inverse(boxInWorld);
            Vector3 pointInBox = HomogenousTransform.Apply(worldInBox, pointInWorld);
            
            // checking every dimension within the box's local coordinate system
            bool withinRangeX = (pointInBox.X < (1.0 / 2.0) * this.length) && (pointInBox.X > (-1.0 / 2.0) * this.length);
            bool withinRangeY = (pointInBox.Y < (1.0 / 2.0) * this.width) && (pointInBox.Y > (-1.0 / 2.0) * this.width);
            bool withinRangeZ = (pointInBox.Z < (1.0 / 2.0) * this.height) && (pointInBox.Z > (-1.0 / 2.0) * this.height);

            // the box contains the point only when each dimension of the coordinates
            // of the point is within the range of the box in the corresponding dimension
            return withinRangeX && withinRangeY && withinRangeZ;
        }
    }
}
